package ar.edu.untref.robotica.simulador;

import java.awt.event.KeyEvent;
import java.awt.event.KeyListener;
import java.awt.event.MouseWheelEvent;
import java.awt.event.MouseWheelListener;
import java.util.Timer;
import java.util.TimerTask;

public class Simulador extends TimerTask implements KeyListener,
		MouseWheelListener {

	private Graficador graficador;
	private Escenario escenario;
	private Robot robot;

	private boolean estaFrenando = false;
	private double fuerzaDeFrenado = 50;
	private double deltaDeFrenado;

	private boolean estaAcelerando = false;
	private double velocidadFinalDeseada;
	private double deltaDeAceleracion = 0.01;
	
	private boolean girar = false;
	private boolean gire = false;

	public Simulador(Escenario escenario) {

		this.escenario = escenario;
	}

	public void inicializar() {

		graficador = new Graficador(this.escenario);
		graficador.addKeyListener(this);
		graficador.addMouseWheelListener(this);
		graficador.setFocusable(true); // Para que ande el teclado.

		this.robot = new Robot(escenario, Configuracion.instancia()
				.getAltoRobot(), Configuracion.instancia().getAnchoRobot());

		Timer timer = new Timer();

		int fps = 60;
		double period = ((double) 1 / fps) * 1000;
		timer.scheduleAtFixedRate(this, 0, (long) period);
	}

	public void keyPressed(KeyEvent e) {

		switch (e.getKeyChar()) {

		case 'w':
			if (puedeAvanzar()) {
				this.acelerarRobot(1);
			}
			break;

		case 'x':
			this.frenarRobot();
			break;

		case 's':
			if (this.puedeRetroceder()) {
				this.acelerarRobot(-1);
			}
			break;

		case 'd':
			this.robot.aumentarAngulo();
			// if(this.graficador.choque()){ //SUPER LENTO
			// this.robot.disminuirAngulo();
			// }
			break;

		case 'a':
			this.robot.disminuirAngulo();
			// if(this.graficador.choque()){ //SUPER LENTO
			// this.robot.aumentarAngulo();
			// }
			break;
			
		case 'z':
			girar=true;
			break;
			
		case 'c':
			this.robot.getRuedaI().acelerar(+1);
			this.robot.getRuedaD().acelerar(-1);
			break;

		case '+':
			this.graficador.aumentarZoom();
			break;

		case '-':
			this.graficador.disminuirZoom();
			break;

		case 'i':
			System.out.println("Obstaculo a "
					+ robot.getDistanciaObstaculoAdelante(escenario));
			break;

		case 'k':
			System.out.println("Obstaculo a "
					+ robot.getDistanciaObstaculoAtras(escenario));
			break;

		case 'j':
			System.out.println("Obstaculo a "
					+ robot.getDistanciaObstaculo45Izquierda(escenario));
			break;

		case 'l':
			System.out.println("Obstaculo a "
					+ robot.getDistanciaObstaculo45Derecha(escenario));
			break;
		}

	}

	public void keyTyped(KeyEvent e) {
		/* nada que hacer acá */
	}

	public void keyReleased(KeyEvent e) {
		/* nada que hacer acá */
	}

	
	// Logica de navegacion/movimiento

	@Override
	public void run() {

		if (this.robot.getVelocidad() > 0) {
			if (!puedeAvanzar()) {
				this.frenarRobot();
			}

		} else if (this.robot.getVelocidad() < 0) {
			if (!puedeRetroceder()) {
				this.frenarRobot();
			}
		}
		
		System.out.println(robot.getVelocidad());
		
//		if (velocidadDeseadaAlcanzada()) {
//			estaAcelerando = false;
//		}
		
		if(girar){
			this.logicaGiro();
		}

		if (estaAcelerando) {
			this.logicaAceleracion();
		}

		if (estaFrenando) {
			this.logicaFrenado();
		} else {
			this.deltaDeFrenado = Math.abs(robot.getVelocidad()
					/ fuerzaDeFrenado);
		}

		this.reflejarMovimiento();
	}
	

	public boolean puedeAvanzar() {
		boolean puedeAvanzar = true;
		double distanciaSegura = Configuracion.instancia().getDistanciaSegura();
		if (this.robot.getDistanciaObstaculoAdelante(escenario) < distanciaSegura
				|| this.robot.getDistanciaObstaculo45Derecha(escenario) < distanciaSegura
				|| this.robot.getDistanciaObstaculo45Izquierda(escenario) < distanciaSegura) {
			puedeAvanzar = false;
		}
		return puedeAvanzar;
	}

	public boolean puedeRetroceder() {
		boolean puederetroceder = true;
		double distanciaSegura = Configuracion.instancia().getDistanciaSegura();
		if (this.robot.getDistanciaObstaculoAtras(escenario) < distanciaSegura) {
			puederetroceder = false;
		}
		return puederetroceder;
	}

	public void reflejarMovimiento() {

		Posicion posicionRobot = this.escenario.getPosicionRobot();

		double posicionXActual = posicionRobot.getPosicionX();
		double posicionYActual = posicionRobot.getPosicionY();

		double velocidad = this.robot.getVelocidad();
		double angulo = Math.toRadians(this.escenario.getPosicionRobot()
				.getAngulo());

		double posicionXNueva = posicionXActual + velocidad * Math.sin(angulo);
		double posicionYNueva = posicionYActual - velocidad * Math.cos(angulo);

		posicionRobot.setPosicionX(posicionXNueva);
		posicionRobot.setPosicionY(posicionYNueva);

		// if (this.graficador.choque()) { // LENTOOOOOOO
		// posicionRobot.setPosicionX(posicionXActual);
		// posicionRobot.setPosicionY(posicionYActual);
		// System.out.println("CHOQUEEEEE!!!");
		// this.robot.frenar();
		// }

		this.graficador.repaint();
	}

	public void frenarRobot() {
		this.estaFrenando = true;
		this.estaAcelerando = false;
	}

	public void logicaFrenado() {

		if (this.robot.getVelocidad() > 0) {
			robot.getRuedaI().acelerar(robot.getVelocidad() - deltaDeFrenado);
			robot.getRuedaD().acelerar(robot.getVelocidad() - deltaDeFrenado);
		} else if (robot.getVelocidad() < 0) {
			robot.getRuedaI().acelerar(robot.getVelocidad() + deltaDeFrenado);
			robot.getRuedaD().acelerar(robot.getVelocidad() + deltaDeFrenado);
		}

		if (robot.getVelocidad() < 0.01 && robot.getVelocidad() > -0.01) {
			robot.frenar();
			estaFrenando = false;
		}
	}

	//Ordena la aceleracion si es necesario
	public void acelerarRobot(double VelocidadFinal) {
		this.estaFrenando = false;
		this.velocidadFinalDeseada = VelocidadFinal;
		if (!velocidadDeseadaAlcanzada()) {			//sin este puedo seguir acelerando cada vez mas
			this.estaAcelerando = true;
		}
	}

	//Ingresa si el robot esta en modo acelerar, y modifica su velocidad segun corresponda
	//una vez alcanzada la velocidad final, quita el modo aceleracion.
	private void logicaAceleracion() {
		if (this.velocidadFinalDeseada > 0) {
			robot.getRuedaI().acelerar(robot.getVelocidad() + deltaDeAceleracion);
			robot.getRuedaD().acelerar(robot.getVelocidad() + deltaDeAceleracion);
			
		} else if (velocidadFinalDeseada < 0) {
			robot.getRuedaI().acelerar(robot.getVelocidad() - deltaDeAceleracion);
			robot.getRuedaD().acelerar(robot.getVelocidad() - deltaDeAceleracion);
		}

		if (velocidadDeseadaAlcanzada()) {
			estaAcelerando = false;
		}
	}

	private boolean velocidadDeseadaAlcanzada() {
		double margenTolerancia = 0.01;
		boolean velocidadDeseadaFueAlcanzada = false;
		if (robot.getVelocidad() < (velocidadFinalDeseada + margenTolerancia)
				&& robot.getVelocidad() > (velocidadFinalDeseada - margenTolerancia)) {
			velocidadDeseadaFueAlcanzada = true;
		}
		return velocidadDeseadaFueAlcanzada;
	}
	
	private void logicaGiro(){
		if(!gire){
			this.robot.getRuedaI().acelerar(-1);
			this.robot.getRuedaD().acelerar(+1);
			gire=true;
		}else{
			this.robot.getRuedaI().acelerar(0);
			this.robot.getRuedaD().acelerar(0);
			gire=false;
			girar=false;
		}
	}

	@Override
	public void mouseWheelMoved(MouseWheelEvent e) {

		int notches = e.getWheelRotation();

		if (notches < 0) {

			this.graficador.aumentarZoom();

		} else {

			this.graficador.disminuirZoom();
		}

	}

}